#! /usr/bin/env python3
import serial
import rospy
from geometry_msgs.msg import PoseStamped

hight=0
q=0
lastdata=0
landmode=0
errtimes=0
t265_data=''
def t265_cb(data):
    global hight
    global pub
    global t265_data
    global q
    global lastdata
    global errtimes
    t265_data=data
    if(errtimes>=7):
        t265_data.pose.position.z=t265_data.pose.position.z/0.75
        rospy.loginfo(str(t265_data.pose.position.z)+"using T265!!!!!!!!!!")
        pub.publish(t265_data)
        return
    if(hight<=3):
        if(errtimes<=11):
            errtimes+=1
        t265_data.pose.position.z=lastdata/100.0+0.000000011111111
        rospy.loginfo("sensor error once")
    else: 
        rospy.loginfo(t265_data.pose.position.z)
        t265_data.pose.position.z=hight/100.0+0.000000011111111
    if(q<65536 and q>100):
        pub.publish(t265_data)
        rospy.loginfo(hight)
    else:
        rospy.loginfo("quality error")
    lastdata=hight

    
ser=serial.Serial('/dev/ttyS4',115200)
if ser.isOpen():
	rospy.loginfo("Serial port opened successfully.")
	ser.flushInput()
        
rospy.init_node('fake_t265', anonymous=True)
sub=rospy.Subscriber("/t265_data",PoseStamped,t265_cb)
pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=10)
rate = rospy.Rate(20)
#while(not(data[0]==87 and data[1]==87))
while not rospy.is_shutdown():
    #rospy.spin()
    data = ser.read(9)
    if(data[0]==89 and data[1]==89):
        hight=data[2]+256*data[3]
        q=data[4]+256*data[5]
    else:
        ser.read(1)
        rospy.loginfo(data[0])
        q=0
    #print(data)
    #if(q>100 and q<65535):
     #   print(hight)
        #print(q)
        #pub.publish(t265_data)
    #rate.sleep()

